# 在特定的xy坐标下执行z归位
#
# Copyright (C) 2019 Florian Heilmann <Florian.Heilmann@gmx.net>
#
# This file may be distributed under the terms of the GNU GPLv3 license.

class SafeZHoming:
    def __init__(self, config):
        self.printer = config.get_printer()
        # 归位时的xy坐标
        x_pos, y_pos = config.getfloatlist("home_xy_position", count=2)
        self.home_x_pos, self.home_y_pos = x_pos, y_pos
        # 在归位前抬升Z轴的距离（mm）
        self.z_hop = config.getfloat("z_hop", default=0.0)
        # z抬升时的速度
        self.z_hop_speed = config.getfloat('z_hop_speed', 15., above=0.)
        zconfig = config.getsection('stepper_z')
        self.max_z = zconfig.getfloat('position_max', note_valid=False)
        # 归位速度
        self.speed = config.getfloat('speed', 50.0, above=0.)
        # 如果配置了该参数为True，则归位完成后会回到xy坐标之前的位置
        self.move_to_previous = config.getboolean('move_to_previous', False)
        # 加载homing模块
        self.printer.load_object(config, 'homing')
        # 加载gcode模块
        self.gcode = self.printer.lookup_object('gcode')
        # 注册G28归位命令，safe_z_home仅仅是重新调用了homing模块注册的G28命令，并未实现新的归位处理函数
        # homing模块的G28命令会执行XYZ三轴的归位操作
        self.prev_G28 = self.gcode.register_command("G28", None)
        self.gcode.register_command("G28", self.cmd_G28)

        # safe_z_home不能和homing_overrde配置同时存在
        if config.has_section("homing_override"):
            raise config.error("homing_override and safe_z_homing cannot"
                               +" be used simultaneously")

    def cmd_G28(self, gcmd):
        """
        执行G28命令,实际的归位操作是由homing注册的归位函数执行(prev_G28)
        """
        toolhead = self.printer.lookup_object('toolhead')

        # 执行Z抬升
        if self.z_hop != 0.0:
            # Check if Z axis is homed and its last known position
            curtime = self.printer.get_reactor().monotonic()
            kin_status = toolhead.get_kinematics().get_status(curtime)
            pos = toolhead.get_position()

            if 'z' not in kin_status['homed_axes']:
                # z轴未归位则执行抬升
                pos[2] = 0
                # 设置打印头当前所在的z位置就是就是z轴的0位置
                toolhead.set_position(pos, homing_axes=[2])
                toolhead.manual_move([None, None, self.z_hop],
                                     self.z_hop_speed)
                if hasattr(toolhead.get_kinematics(), "note_z_not_homed"):
                    toolhead.get_kinematics().note_z_not_homed()
            elif pos[2] < self.z_hop:
                # z轴已归位，但是z轴当前的位置小于抬升距离，还是执行抬升
                toolhead.manual_move([None, None, self.z_hop],
                                     self.z_hop_speed)

        # 通过解析命令参数来判断需要归位哪几个轴
        need_x, need_y, need_z = [gcmd.get(axis, None) is not None
                                  for axis in "XYZ"]
        if not need_x and not need_y and not need_z:
            need_x = need_y = need_z = True

        # 如果需要的话也归位XY轴
        new_params = {}
        if need_x:
            new_params['X'] = '0'
        if need_y:
            new_params['Y'] = '0'
        if new_params:
            g28_gcmd = self.gcode.create_gcode_command("G28", "G28", new_params)
            self.prev_G28(g28_gcmd)

        # Z归位
        if need_z:
            # 如果XY未归位则报告错误
            curtime = self.printer.get_reactor().monotonic()
            kin_status = toolhead.get_kinematics().get_status(curtime)
            if ('x' not in kin_status['homed_axes'] or
                'y' not in kin_status['homed_axes']):
                raise gcmd.error("Must home X and Y axes first")
            # 记录xy移动之前的位置
            prevpos = toolhead.get_position()
            # 移动到home_xy_position指定的xy坐标
            toolhead.manual_move([self.home_x_pos, self.home_y_pos], self.speed)
            # 发送G28 Z=0命令，执行Z轴归位
            g28_gcmd = self.gcode.create_gcode_command("G28", "G28", {'Z': '0'})
            self.prev_G28(g28_gcmd)
            # z归位已完成，再次执行Z抬升
            if self.z_hop:
                pos = toolhead.get_position()
                if pos[2] < self.z_hop:
                    toolhead.manual_move([None, None, self.z_hop],
                                         self.z_hop_speed)
            # 恢复XY到之前的位置
            if self.move_to_previous:
                toolhead.manual_move(prevpos[:2], self.speed)

def load_config(config):
    return SafeZHoming(config)
